package bcit.nxt.mission;

// TurtleSquare.java
import gin.and.tonic.*;
import lejos.nxt.Button;

public class Missionball implements UltrasonicListener, TouchListener
{

    private DragonRobot robot = new DragonRobot();
    int count=0;

    Missionball()
    {
        UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
        TouchSensor ts = new TouchSensor(SensorPort.S2);
        Motor arm = new Motor(MotorPort.A);
        robot.addPart(us);
        robot.addPart(ts);
        //us.addUltrasonicListener(this,20);
        //  ts.addTouchListener(this);
        arm.setSpeed(20);
        robot.addPart(arm);

        /*Mission 3*/
       /* robot.setTurtleSpeed(50);
        arm.rotateTo(160, true);
        robot.forward(72);
        robot.backward(34);
        arm.rotateTo(25, true);
        robot.forward(18);
        robot.backward(56);
        arm.rotateTo(-185, true);
        * */
        
        /*Mission ball&magnet*/
        robot.forward(745);//051911,let the tail touch the wall//700);
        robot.left(10);
        arm.rotateTo(25,true);
        arm.rotateTo(-10,true);
        //arm.rotateTo(30,true);  //051611
        robot.backward(10);//051611
        robot.right(70);//051611//45);
        arm.rotateTo(20,true);
        robot.forward(35);//051611 //55);
        robot.right(18);
        //robot.forward(20);
        arm.rotateTo(-60,true);
        arm.getMot().lock(60);
        //Tools.delay(1000);
        arm.rotateTo(40,true);
        robot.left(60);
        //arm.rotateTo(-80,true);
        robot.backward(800);
        robot.exit();
        Button.waitForPress();
        

        /*Mission magnet*/
        /*robot.forward(660);
        robot.right(60);
        arm.rotateTo(-60,true);
        arm.rotateTo(60,true);
        robot.left(60);
        robot.backward(660);*/

        /*Misson module*/
        //robot.forward(1600);
        //robot.left(30);




        /*cross for NANO to POWER base*/
       /*robot.setTurtleSpeed(50);
         robot.forward(211);
         robot.left(70);
         robot.setTurtleSpeed(80);
         robot.forward(170);
         robot.setTurtleSpeed(50);
         robot.left(45);
         robot.forward(84);
         robot.right(45);*/
         //robot.forward(100);
        // robot.exit();


        /*from Power to NANO*/
        //robot.setTurtleSpeed(80);
        //robot.getGear().setSpeed(80);
        //robot.forward(1000);
        //robot.right(360);
        //robot.forward(100);

        
        //robot.left(360);
        //robot.forward(100);

        /*robot.getGear().right();
         robot.forward(422);
         robot.setTurtleSpeed(80);
         robot.right(90);
         robot.getGear().forward();*/


         /*sensor*/
        /* robot.getGear().forward();*/


    }

    public void far(SensorPort port, int level)
    {
        robot.getGear().forward();
    }

    public void near(SensorPort port, int level)
    {
      if(count==2)
      {
       count++;
       return;
          
      }

      if(count>=3)
      {
          count++;
          robot.right(90);
      }

       
       
       if(count<2)
       {
           count++;
           robot.left(90);
        }
      if(count==2)
      {
           robot.forward(63);
           robot.right(90);
           robot.forward(40);
           robot.right(90);
           robot.getGear().forward();
       }

    }

    public void pressed(SensorPort port)
    {
        robot.getGear().forward(200);
        robot.getGear().right();

    }

    public void released(SensorPort port)
    {
        robot.getGear().backward();
    }

    public static void main(String[] args)
    {

        new Missionball();
    }
}
